#include <iostream>
#include <map>
#include <fstream>
#include "Eigen/Dense"
#include "utility.h"
void assemble(Eigen::MatrixXd& k, Eigen::MatrixXd K0, int e1, int e2, int e3, int e4);

Eigen::MatrixXd cal_assem(const Eigen::MatrixXd K);

int main(){
    Eigen::MatrixXd K1(8,8),K2(8,8),K3(8,8),K4(8,8);
    K1 = calculateK(0,0,0.02,0,0.02,0.005,0,0.005);
    K2 = calculateK(0.02,0,0.04,0,0.04,0.005,0.02,0.005);
    K3 = calculateK(0,0.005,0.02,0.005,0.02,0.01,0,0.01);
    K4 = calculateK(0.02,0.005,0.04,0.005,0.04,0.01,0.02,0.01);
    
    //K is the stiffness matrix
    Eigen::MatrixXd  K = Eigen::MatrixXd::Zero(18,18);
    assemble(K,K1,1,2,5,4);
    assemble(K,K2,2,3,6,5);
    assemble(K,K3,4,5,8,7);
    assemble(K,K4,5,6,9,8);
    //cout << K << endl;

    Eigen::MatrixXd k_cal(12,12);
    k_cal = cal_assem(K);

    Eigen::VectorXd F(18);
    Eigen::VectorXd F_cal(12);
    for(int i=0;i<12;i++){
        F_cal(i) = 0;
    }
    F_cal(9) = -200; F_cal(11) = -100;
    Eigen::VectorXd result(12);
    result = k_cal.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(F_cal);
    //cout << result << endl;
    Eigen::VectorXd displacement(18);
    displacement(0) = 0;
    displacement(1) = 0;
    displacement(2) = result(0);
    displacement(3) = result(1);
    displacement(4) = result(2);
    displacement(5) = result(3);
    displacement(6) = 0;
    displacement(7) = 0;
    displacement(8) = result(4);
    displacement(9) = result(5);
    displacement(10) = result(6);
    displacement(11) = result(7);
    displacement(12) = 0;
    displacement(13) = 0;
    displacement(14) = result(8);
    displacement(15) = result(9);
    displacement(16) = result(10);
    displacement(17) = result(11);
    
    ofstream os("displacement.txt");
    for(int i=0;i<18;i++){
        os << displacement(i) << endl;
    }
    os.close();
    Eigen::VectorXd stress(3);
    Eigen::VectorXd strain(3);
    Eigen::MatrixXd stress_Gauss(4,3);
    Eigen::MatrixXd D(3,3);
    Eigen::MatrixXd B(3,8);
    Eigen::VectorXd ele3_displace(8);
    ele3_displace(0) = displacement(6);
    ele3_displace(1) = displacement(7);
    ele3_displace(2) = displacement(8);
    ele3_displace(3) = displacement(9);
    ele3_displace(4) = displacement(14);
    ele3_displace(5) = displacement(15);
    ele3_displace(6) = displacement(12);
    ele3_displace(7) = displacement(13);
    D = calculateD();
    B = calculateB(0,0.005,0.02,0.005,0.02,0.01,0,0.01,0.57735,0.57735);
    strain = B*ele3_displace;
    stress = D*strain;
    for(int i=0;i<3;i++)
    stress_Gauss(0,i) = stress(i);

    B = calculateB(0,0.005,0.02,0.005,0.02,0.01,0,0.01,0.57735,-0.57735);
    strain = B*ele3_displace;
    stress = D*strain;
    for(int i=0;i<3;i++)
    stress_Gauss(1,i) = stress(i);

    B = calculateB(0,0.005,0.02,0.005,0.02,0.01,0,0.01,-0.57735,0.57735);
    strain = B*ele3_displace;
    stress = D*strain;
    for(int i=0;i<3;i++)
    stress_Gauss(2,i) = stress(i);


    B = calculateB(0,0.005,0.02,0.005,0.02,0.01,0,0.01,-0.57735,-0.57735);
    strain = B*ele3_displace;
    stress = D*strain;
    for(int i=0;i<3;i++)
    stress_Gauss(3,i) = stress(i);

    ofstream os1("stress.txt");
    for(int i=0;i<4;i++){
        for(int j=0;j<3;j++){
            os1 << stress_Gauss(i,j) << " ";
        }
        os1 << endl;
    }
    os1.close();
    return 0;
}

void assemble(Eigen::MatrixXd& k, Eigen::MatrixXd K0, int e1, int e2, int e3, int e4){
    int e[4];
    e[0] = e1; e[1] = e2; e[2] = e3; e[3] = e4;
    for(int i=0;i<4;i++){
        for(int j=0;j<4;j++){
            k(2*e[i]-2,2*e[j]-2) += K0(2*i,2*j);
            k(2*e[i]-2,2*e[j]-1) += K0(2*i,2*j+1);
            k(2*e[i]-1,2*e[j]-2) += K0(2*i+1,2*j);
            k(2*e[i]-1,2*e[j]-1) += K0(2*i+1,2*j+1);
        }
    }
}

Eigen::MatrixXd cal_assem(const Eigen::MatrixXd K){
    Eigen::MatrixXd  k_cal = Eigen::MatrixXd::Zero(12,12);
    map<int, int> ktok_cal;
    ktok_cal[0] = 2;
    ktok_cal[1] = 3;
    ktok_cal[2] = 4;
    ktok_cal[3] = 5;
    ktok_cal[4] = 8;
    ktok_cal[5] = 9;
    ktok_cal[6] = 10;
    ktok_cal[7] = 11;
    ktok_cal[8] = 14;
    ktok_cal[9] = 15;
    ktok_cal[10] = 16;
    ktok_cal[11] = 17;

    for(int i=0;i<12;i++){
        for(int j=0;j<12;j++){
            k_cal(i,j) = K(ktok_cal[i],ktok_cal[j]);
        }
    }
    return k_cal;
}